package classes;

import java.util.ArrayList;
import java.util.List;

import logger.Logger;


public class Algebra {
	//
	// The methods provided here are NOT efficient.
	// Some methods call other methods invoking multiple passes through the data.
	// do not use the for large number of vectors or high dimensional spaces
	//
	// Note: all angular measurements are assumed to be in radians
	//
	@SuppressWarnings("unused")
	private static float[][] toMatrix(List<float[]> L) {
		float r[][] = new float[L.size()][];
		int	i, j;
		for(i=0; i<L.size(); i++) {
			r[i] = L.get(i);
		}
		return r;
	}
	@SuppressWarnings("unused")
	private static float[] toVector(List<Float> L) {
		float r[] = new float[L.size()];
		int	i;
		for(i=0; i<L.size(); i++) {
			r[i] = L.get(i).floatValue();
		}
		return r;
	}
	private static int[] toArray(List<Integer> L) {
		int r[] = new int[L.size()];
		int	i;
		for(i=0; i<L.size(); i++) {
			r[i] = L.get(i).intValue();
		}
		return r;
	}
	
	public static void printMatrix(float m[][]) {
		
		int row;
		System.out.println("*********************************");
		
		for(row=0; row < m.length; row++) {
			printVector(m[row]);
		}
		System.out.println("*********************************");
	}
	public static void printVector(float v[]) {
		int col;
		for(col = 0; col < v.length; col++) {
			System.out.printf("[ %7.2f ]", v[col]);
		}
		System.out.println("");
	}
	public static void printVector(int v[]) {
		int col;
		for(col = 0; col < v.length; col++) {
			System.out.printf("[ %7d ]", v[col]);
		}
		System.out.println("");
	}
	
	
	public static String	matrixString(float m[][]) {
		int		row;
		String	output = "";
		
		output += "*********************************\n";
		for(row=0; row < m.length; row++) {
			output += vectorString(m[row]) + "\n";
		}
		output += "*********************************\n";
		return output;
	}
	public static String	vectorString(float v[]) {
		int col;
		String	output = "";
		
		for(col = 0; col < v.length; col++) {
			output += String.format("[ %7f ]", v[col]);
		}
		return output;
	}

	public float[]	subVector(int start, int size, float K[]) {
		float	N[] = new float[size];
		int		i;
		for(i=0; i<size; i++) {
			N[0] = K[start+size];
		}
		return N;
	}
	
	public static int[]	selectMatchingRows(int col, float value, float M[][]) {
		ArrayList<Integer> L = new ArrayList<Integer>();
		int	i;
		for(i=0; i<numRows(M); i++) {
			if (M[i][col]==value) {
				L.add(new Integer(i));
			}
		}
		return toArray(L);
	}
	public static int[]	selectMatchingCols(int row, float value, float M[][]) {
		ArrayList<Integer> L = new ArrayList<Integer>();
		int	i;
		for(i=0; i<numCols(M); i++) {
			if (M[row][i]==value) {
				L.add(new Integer(i));
			}
		}
		return toArray(L);
	}
	
	public static int[]	lineClosest(float data[], float target) {
		int	B[] = twoClosestButDifferent(data, target);
		if (data[B[0]] > data[B[1]]) {
			return new int[] { B[1], B[0] };
		} else {
			return B;
		}
	}
	public static int[] twoClosestButDifferent(float data[], float target) {
		//
		// Returns indicies of the two closest points in the data set
		// This will NOT return them sorted by distance from the point
		//
		int		m0 = -1, m1 = -1;
		float	d0 = Float.POSITIVE_INFINITY, d1 = Float.POSITIVE_INFINITY, di = Float.POSITIVE_INFINITY;
		float	v0 = Float.POSITIVE_INFINITY, vi = Float.POSITIVE_INFINITY;
		int		i;
		int		found = 0;
		
		for(i=0; i<data.length; i++) {
			
			di = Math.abs(data[i]-target);
			vi = data[i];
			
			if (found==0) {
				m0 = i;
				d0 = di;
				found = 1;
			} else {
				if (vi != v0 && (found==1 || di <= d0)) {
					m1 = m0;
					d1 = d0;
					m0 = i;
					d0 = di;
					v0 = vi;
					found = 2;
				} else if (found==1 || di < d1) {
					m1 = i;
					d1 = di;
					found = 2;
				}
			}
		}
		return new int[] { m0, m1 };
		
	}
	
	public static float[] appendData(float d1[], float d2[]) {
		float	r[] = new float[d1.length+d2.length];
		int		i;
		for(i=0; i<d1.length; i++) {
			r[i] = d1[0];
		}
		for(i=0; i<d2.length; i++) {
			r[i + d1.length] = d2[i];
		}
		return r;
	}
	public static int[] appendData(int d1[], int d2[]) {
		int	r[] = new int[d1.length+d2.length];
		int		i;
		for(i=0; i<d1.length; i++) {
			r[i] = d1[0];
		}
		for(i=0; i<d2.length; i++) {
			r[i + d1.length] = d2[i];
		}
		return r;
	}
	
	public static float[][]	selectRows(int rows[], float M[][]) {
		float	N[][] = new float[rows.length][numCols(M)];
		int		i;
		for(i=0; i<rows.length; i++) {
			N[i] = getRow(rows[i],M);
		}
		return N;
	}
	public static float[][] selectCols(int cols[], float M[][]) {
		float	N[][] = new float[numRows(M)][cols.length];
		int		i, j;
		for(j=0; j<numRows(N); j++) {
			for(i=0; i<cols.length; i++) {
				N[j][i] = M[j][cols[i]];
			}
		}
		return N;
	}
	
	public static void setRow(int row, float v[], float m[][]) {
		int col;
		for(col=0; col< numCols(m); col++) {
			m[row][col] = v[col];
		}
	}
	public static void setCol(int col, float v[], float m[][]) {
		int row;
		for(row=0; row<numRows(m); row++) {
			m[row][col] = v[row];
		}
	}
	public static float[] getCol(int col, float m[][]) {
		float	v[] = new float[numRows(m)];
		int		i;
		for(i=0; i<numRows(m); i++) {
			v[i] = m[i][col];
		}
		return v;
	}
	public static float[] getRow(int row, float m[][]) {
		float	v[] = new float[numCols(m)];
		int		i;
		for(i=0; i<numCols(m); i++) {
			v[i] = m[row][i];
		}
		return v;
	}
	
	public static float[][] newMatrix(int numRows, int numCols) {
		return new float[numRows][numCols];
	}
	
	public static float[] copy(float v[]) {
		int		i;
		float	r[] = new float[v.length];
		
		for(i=0; i<v.length; i++) {
			r[i] = v[i];
		}
		return r;
	}
	public static float[][] copy(float matrix[][]) {
		return getRegion(0,numRows(matrix),0,numCols(matrix),matrix);
	}
	public static float[][]	getCols(int start, int size, float matrix[][]) {
		return getRegion(0,numRows(matrix),start,size,matrix);
	}
	public static float[][]	getRows(int start, int size, float matrix[][]) {
		return getRegion(start,size,0,numCols(matrix),matrix);
	}
	public static float[][] getRegion(int startRow, int rowSize, int startCol, int colSize, float matrix[][]) {
		float	M[][] = new float[rowSize][colSize];
		int	row, col;
		
		for(row=0; row<rowSize; row++) {
			for(col=0; col<colSize; col++) {
				M[row][col] = matrix[startRow+row][startCol+col];
			}
		}
		return M;
	}
	
	public static float sum(float data[]) {
		int		i;
		float	sum = 0;
		
		for(i=0; i<data.length; i++) {
			sum += data[i];
		}
		return sum;
	}

	public static float	linear(float x, float y, float dx, float dy, float Tx) {
		if (dx==0) {
			return Float.POSITIVE_INFINITY;
		} else {
			float	m = dy/dx;
			float	b = y-m*x;
			float	Ty = m*Tx+b;
//			System.out.printf("**********************************************************\n");
//			System.out.printf("     dy                          %5.2f                    \n", dy);    
//			System.out.printf("Y = ---- X + B    ==     %5.2f = ------ ( %5.2f ) + %5.2f \n", Ty,Tx,b);  
//			System.out.printf("     dx                          %5.2f                    \n", dx);
//			System.out.printf("**********************************************************\n");
			return	Ty;
		}
	}
	public static float[] linear(float v1[], float v2[], float known, int keyIdx, float keyValue) {
		//
		// This will perform linear extrapolation of a vector from one to the next
		//
		int		i;
		float	r[] = new float[v1.length];
		
		for(i=0; i<r.length; i++) {
			if (i!=keyIdx) {
				float	x = v1[i];
				float	y = v1[keyIdx];
				float	dx = v2[i] - v1[i];
				float	dy = v2[keyIdx] - v1[keyIdx];
				r[i] = linear(x,y,dx,dy,keyValue);
			} else {
				r[i] = keyValue;
			}
		}
		
		return r;
	}
	
	public static float[] setVector(int numDimensions, float value) {
		float	v[] = new float[numDimensions];
		int		i;
		for(i=0; i<numDimensions; i++) {
			v[i] = value;
		}
		return v;
	}
	public static float[] zeroVector(int numDimensions) {
		return setVector(numDimensions, 0f);
	}

	public static float average(float v[]) {
		return sum(v) / v.length;
	}
	public static float[] square(float v[]) {
		return mul(v,v);
	}
	public static float[] add(float v1[], float v2[]) {
		int 	i;
		float	r[] = new float[v1.length];
		
		for(i=0; i<r.length; i++) {
			r[i] = v1[i] + v2[i];
		}
		return r;
	}
	public static float[] scale(float v[], float scalar) {
		float	r[] = new float[v.length];
		int		i;
		
		for(i=0; i<r.length; i++) {
			r[i] = v[i] * scalar;
		}
		return r;
	}
	public static float[] diff(float v1[], float v2[]) {
		return add(v1, scale(v2,-1f));
	}
	public static float dotProduct(float v1[], float v2[]) {
		float	sum = 0f;
		int		i;
		
		for(i=0; i<v1.length; i++) {
			sum += v1[i] * v2[i];
		}
		return sum;
	}
	public static boolean isOrthagonal(float v1[], float v2[]) {
		return (dotProduct(v1,v2)==0f);
	}
	public static float magnitude(float v[]) {
		return (float) Math.sqrt(sum(square(v)));
	}
	public static float theta(float v1[], float v2[]) {
		return (float) Math.acos( dotProduct(v1,v2) / (magnitude(v1) * magnitude(v2)) );
	}
	public static float[] mul(float v1[], float v2[]) {
		float	v3[] = new float[v1.length];
		int		i;
		
		for(i=0; i<v3.length; i++) {
			v3[i] = v1[i] * v2[i];
		}
		return v3;
	}
	public static float[] project(float v1[], float v2[]) {
		//
		// This will find the projection of v1 onto v2
		//
		float	dot			= dotProduct(v1, v2);
		float	magV2		= magnitude(v2);
		float	projRatio	=  dot / (magV2*magV2);

		return	scale(v2, projRatio); 
	}
	
	
	public static float[] sum(float data[][]) {
		//
		// This will return the average vector of a cloud of vectors
		//
		int		dimension, vector;
		float	sum[] = new float[data[0].length];
		
		for(dimension = 0; dimension < sum.length; dimension++) {
			sum[dimension] = 0f;
		}
		
		for(vector=0; vector<data.length; vector++) {
			sum = add(sum, data[vector]);
		}
		
		return sum;
	}
	public static float[] centroid(float data[][]) {
		float	sum[]	= sum(data);
		float	avg[]	= scale(sum, 1/data[0].length);
		
		return avg;
	}
	public static float[] mostOrthagonal(float data[][], float v[]) {
		float	best[] = null;
		float	thetaBest = 0f;
		int		i;
		
		for(i=0; i<data.length; i++) {
			if (best==null) {
				best = data[i];
				thetaBest = Math.abs(theta(v,best));
			} else {
				float	thetaI = Math.abs(theta(v,data[i]));
				if (thetaI > thetaBest) {
					thetaBest = thetaI;
					best = data[i];
				}
			}
		}
		
		return best;
	}
	public static float[] mostParallel(float data[][], float v[]) {
		float	best[] = null;
		float	thetaBest = 0f;
		int		i;
		
		for(i=0; i<data.length; i++) {
			if (best==null) {
				best = data[i];
				thetaBest = Math.abs(theta(v,best));
			} else {
				float	thetaI = Math.abs(theta(v,data[i]));
				if (thetaI < thetaBest) {
					thetaBest = thetaI;
					best = data[i];
				}
			}
		}
		
		return best;
	}

	public static float[][] upperTriangular(float origonal[][]) {
		//
		// This will convert a matrix to upper triangular form
		// See method solve the matrix for key variables
		// This copies the origonal matrix and then alters the copy
		//
		float	matrix[][] = copy(origonal);
		int 	row1, row2;
		int		nRows = numRows(matrix);
		
		for(row1=0; row1<nRows; row1++) {
			float	leadC = matrix[row1][row1];
			
			//
			// Ensure there that leadC != 0 if possible 
			//
			for(row2 = row1+1;	row2 < nRows && leadC==0f; row2 ++) {
				float	tmp[] = matrix[row1];
				matrix[row1] = matrix[row2];
				matrix[row2] = tmp;
				leadC = matrix[row1][row1];
			}
			
			//
			// If lead c != 0 modify
			//
			if (leadC!=0) {
				if (leadC!=1) {
					matrix[row1] = scale(matrix[row1], 1f/leadC);
					leadC = matrix[row1][row1];
				}
			}
			
			//
			// Change the remainder
			//
			for(row2 = row1+1; row2 < nRows; row2++) {
				matrix[row2] = add(matrix[row2], scale(matrix[row1], -matrix[row2][row1]));
			}
		}
		return matrix;
	}
	public static float[] solve(float origonal[][], int outputColumn) {
		//
		// Note that numRows = numCols-1
		// Converts matrix to gausian form
		// Does not destroy the origonal
		//
		float	matrix[][] = copy(origonal);
		int		row1, row2;
		float	v[] = new float[numRows(matrix)];
		upperTriangular(matrix);

		for(row1=numRows(matrix)-1; row1 >= 0; row1--) {
			for(row2=0; row2<row1; row2++) {
				matrix[row2] = add(matrix[row2], scale(matrix[row1], -matrix[row2][row1]));
			}
			v[row1] = matrix[row1][outputColumn];
		}
		
		return v;
	}
	
	public static float[][] zeroMatrix(int numRows, int numCols) {
		return setMatrix(numRows,numCols,0f);
	}
	public static float[][] setMatrix(int numRows, int numCols, float value) {
		float	M[][] = new float[numRows][numCols];
		int	col, row;
		for(row=0; row<numRows; row++) {
			for(col=0; col<numCols; col++) {
				M[row][col] = value;
			}
		}
		return M;
	}
	public static float[][] identityMatrix(int dimension) {
		//
		// Returns identity matrix diagonal = 1f rest = 0f
		//
		float	M[][] = zeroMatrix(dimension,dimension);
		int		i;
		for(i=0; i<dimension; i++) {
			M[i][i] = 1f;
		}
		return M;
	}

	public static float[][] transpose(float matrix[][]) {
		//
		// Flips the value over the diagnonal
		//
		int		nRows = numRows(matrix);
		int		nCols = numCols(matrix);
		float	M[][] = new float[nCols][nRows];
		int		col, row;
		
		for(row=0; row<nRows; row++) {
			for(col=0; col<nCols; col++) {
				M[col][row] = matrix[col][row];
			}
		}
		return M;
	}
	public static float[][] appendCols(float M1[][], float M2[][]) {
		//
		// Attaches two matricies such that rows line up.
		// Number of rows must be the same
		//
		int		row, col;
		float	M3[][] = new float[numRows(M1)][numCols(M1) + numCols(M2)];
		
		for(row=0; row<numRows(M1); row++) {
			for(col=0; col<numCols(M1); col++) {
				M3[row][col] = M1[row][col];
			}
			for(col=0; col<numCols(M2); col++) {
				M3[row][col+numCols(M1)] = M2[row][col]; 
			}
		}
		return M3;
	}
	public static float[][] appendRows(float M1[][], float M2[][]) {
		//
		// Attaches two matricies such that cols line up.
		// Number of cols must be the same
		//
		int		row, col;
		float	M3[][] = new float[numRows(M1)][numCols(M1) + numCols(M2)];
		
		for(col=0; col<numCols(M1); col++) {
			for(row=0; row<numRows(M1); row++) {
				M3[row][col] = M1[row][col];
			}
			for(row=0; row<numRows(M2); row++) {
				M3[row+numRows(M1)][col] = M2[row][col]; 
			}
		}
		return M3;
	}
	
	public static int numRows(float matrix[][]) {
		return matrix.length;
	}
	public static int numCols(float matrix[][]) {
		if (numRows(matrix)>0) {
			return matrix[0].length;
		} else {
			return 0;
		}
	}

	public static float[][] solveRemainder(float origonal[][]) {
	//
	// Note that numRows = numCols-1
	// Assumes initial numCols(matrix) > numRows(matrix)
	// Assumes that column numRows+1 contains initial target value
	//
		float	matrix[][] = copy(origonal);
		int	row1, row2;
		float	v[] = new float[numRows(matrix)];

		matrix = upperTriangular(matrix);
	
		for(row1=numRows(matrix)-1; row1 >= 0; row1--) {
			for(row2=0; row2<row1; row2++) {
				matrix[row2] = add(matrix[row2], scale(matrix[row1], -matrix[row2][row1]));
			}
			v[row1] = matrix[row1][row1+1];
		}
		
		return getCols(numRows(matrix),numCols(matrix)-numRows(matrix), matrix);
	}
	public static float[][] inverse(float matrix[][]) {
		//
		// This calculates the inverse matrix by solving
		// peforming gaussian elimination
		//
		float Mi[][] = identityMatrix(numRows(matrix));
		float Ma[][] = appendCols(matrix, Mi);
		return solveRemainder(Ma);
	}
	
	public static boolean	isEqual(float M1[][], float M2[][]) {
		//
		// Checks to see if two matricies are equal
		//
		if (numRows(M1)==numRows(M2) && numCols(M1) == numCols(M2)) {
			int	row, col;
			
			for(row=0; row<numRows(M1); row++) {
				for(col=0; col<numCols(M1); col++) {
					float v1 = M1[row][col];
					float v2 = M2[row][col];
					
					int	cmp = Float.toString(v1).compareTo(Float.toString(v2));
					if ( cmp!=0 ) {
						System.out.println("Mismatch at row=" + row + " , col=" + col);
						System.out.printf(" M1 = %f, M2 = %f\n" , M1[row][col], M2[row][col]);
						System.out.println(" cmp == " + cmp);
						return false;
					}
				}
			}
			return true;
		} else {
			System.out.println("Dimensions don't match");
			return false;
		}
	}

	public static float[]	getDistances(float data[][], float v[]) {
		//
		// This will treat each row as a vector and return a new vector that contains
		// all of the distances
		//
		float	d[] = new float[numRows(data)];
				int i;
		
		for(i=0; i<numRows(data); i++) {
			d[i] = magnitude(diff(v, data[i]));
		}
		return d;
	}
	public static float[][] sort(int d, float m[][]) {
		//
		// This will sort the matrix on one of the given dimensions
		// this method is non-destructive
		//
		int i, j;
		float	n[][] = copy(m);
		float	tmp[];
		boolean	done = false;
		
		for(i=0; i<numRows(n) && !done; i++) {
			done = true;
			for(j=i+1; j<numRows(n); j++) {
				if (n[i][d] > n[j][d]) {
					tmp = n[i];
					n[i] = n[j];
					n[j] = tmp;
					done = false;
				}
			}
		}
		
		return n;
	}
	
	public static float[][] toColumn(float v[]) {
		float	m[][] = new float[v.length][1];
		int		i;
		for(i=0; i<v.length; i++) {
			m[i][0] = v[i];
		}
		return m;
	}
	public static float[][] toRow(float v[]) {
		float	m[][] = new float[1][v.length];
		int		i;
		for(i=0; i<v.length; i++) {
			m[0][i] = v[i];
		}
		return m;
	}
	
	public static float[][] orderedByDistance(float V[], float M[][]) {
		//
		// If M is rows*cols then v = 1xcols
		// returns matrix rows*(cols+1)
		//
		return sort(numCols(M),	appendCols(	M, toColumn( getDistances( M, V	) ) ) );
	}
	
	public static float linearEx(float K[], float matrix[][]) throws Exception {
		return linearEx(0,K,matrix);
	}
	
	public static float	linearEx(int depth, float K[], float matrix[][]) throws Exception {
		//
		// This method is very complicated. But here goes.
		// 
		//
		int		M0[];
		int		M1[];
		float	matrix0[][];
		float	matrix1[][];
		int		B[];
		float	X[], Y[];
		float	x0, x1;
		float	y0, y1;
		float	z;
		float	known;
		
		known = K[depth];
		
		if (depth<K.length-1) {
			//
			// We are on our last dimension of K
			// Retrieve the two closest x values to form the line from this dimension
			//
			X = getCol(depth,matrix);
			
			if (X.length>1) {
				//
				// There is two data points to linear extrapolation from. Continue
				//
				
				B = Algebra.lineClosest(X,known);
				
				// Remember the two values
				x0 = X[B[0]];
				x1 = X[B[1]];
				
				// Find the matching rows for x0 and linear extrapolate that matrix
				M0 = Algebra.selectMatchingRows(depth, x0, matrix);
				matrix0 = Algebra.selectRows(M0, matrix);
				y0 = Algebra.linearEx(depth+1, K, matrix0);
		
				// Check if slope is infinite then we return whatever y0 is
				if (x0==x1) {
					return y0;
//					throw new Exception("Error in Algebra.linearEx() at depth = " + depth + " : dx=0 Infinite slope for the line!");
				}
				
				// Find the matching rows for x1 and linear extrapolate that matrix
				M1 = Algebra.selectMatchingRows(depth, x1, matrix);
				matrix1 = Algebra.selectRows(M1, matrix);
				y1 = Algebra.linearEx(depth+1, K, matrix1);
				
				// Take the two results and linear extrapolate a new result
				System.out.printf("*** z at depth %d/%d\n",depth,K.length);
				z = Algebra.linear(x0, y0, x1-x0, y1-y0, known);
			} else {
				z = 0f;
			}
			
			// Return the extrapolated result
			return z;
		} else {
			//
			// Retrieve the two closest x values to form the line from this dimension
			//
			X = getCol(depth,matrix);
			Y = getCol(depth+1, matrix);

			if (X.length>1) { 
				B = Algebra.lineClosest(X,K[depth]);
				// Remember the two values
				x0 = X[B[0]];
				x1 = X[B[1]];
				
				// Find the matching y0 values for x0 and x1
				y0 = Y[B[0]];
				
				// Check if slope is infinite then we return whatever y0 is
				if (x0==x1) { 
					throw new Exception("Error in Algebra.linearEx() at depth = " + depth + " : dx=0 Infinite slope for the line!");
				}
				
				// Find the matching rows for x1 and linear extrapolate that matrix
				y1 = Y[B[1]];

				// Take the two results and linear extrapolate a new result
				System.out.printf("*** z at depth %d/%d\n",depth,K.length-1);
				z = Algebra.linear(x0, y0, x1-x0, y1-y0, known);
				
			} else if (X.length==1 && K[depth]== X[0]) {
				return Y[0];
			} else {
				throw new Exception("Error in Algebra.linearEx() at depth = " + depth + " : Not enough data for linear extrapolation");
			}
				
			// Return the extrapolated result
			return z;
			
		}
	}
	
	float[][] add(float M1[][], float M2[][]) throws NumberFormatException {
		if (M1==null)	{	throw new NumberFormatException("Cannot multiply a null matrix (M1)");	}
		if (M2==null)	{	throw new NumberFormatException("Cannot multiply a null matrix (M1)");	}
		
		int	Nrows1 = numRows(M1);
		if (Nrows1==0) {	
			throw new NumberFormatException("Matrix M1 has no rows");	
		}
		
		int	Nrows2 = numRows(M2);
		if (Nrows2==0) {	
			throw new NumberFormatException("Matrix M2 has no rows");	
		}
		
		int	Ncols1 = numCols(M1);
		if (Ncols1 < 1)	{	
			throw new NumberFormatException("Matrix M1 has no cols");	
		}
		
		int	Ncols2 = numCols(M2);
		if (Ncols2 < 1)	{	
			throw new NumberFormatException("Matrix M2 has no cols");	
		}
		
		if (Ncols1!=Ncols2) {
			throw new NumberFormatException("Matrix M1 cols (" + Ncols1 +") != M2 cols (" + Ncols2 + ")");
		}
		
		if (Nrows1!=Nrows2) {
			throw new NumberFormatException("Matrix M1 rows (" + Nrows1 +") != M2 rows (" + Nrows2 + ")");
		}
		
		int col, row;
		float	M3[][] = new float[Ncols1][Ncols2];
		for(row=0; row<Nrows1; row++) {
			for(col=0; col<Ncols1; col++) {
				M3[row][col] = M1[row][col] + M2[row][col];
			}
		}
		return M3;
	}
	
	float[][] multiply(float M1[][], float M2[][]) throws NumberFormatException {
		//
		// This will perform Matrix multiplication. If
		// the dimensions are not valid then an exception is thrown
		//
		
		if (M1==null)	{	throw new NumberFormatException("Cannot multiply a null matrix (M1)");	}
		if (M2==null)	{	throw new NumberFormatException("Cannot multiply a null matrix (M1)");	}
		
		int	Nrows1 = numRows(M1);
		if (Nrows1==0) {	
			throw new NumberFormatException("Matrix M1 has no rows");	
		}
		
		int	Nrows2 = numRows(M2);
		if (Nrows2==0) {	
			throw new NumberFormatException("Matrix M2 has no rows");	
		}
		
		int	Ncols1 = numCols(M1);
		if (Ncols1 < 1)	{	
			throw new NumberFormatException("Matrix M1 has no cols");	
		}
		
		int	Ncols2 = numCols(M2);
		if (Ncols2 < 1)	{	
			throw new NumberFormatException("Matrix M2 has no cols");	
		}

		if (Ncols1 != Nrows2) {	
			throw new NumberFormatException("M1 cols (" + Ncols1 + ") dont match M2 rows (" + Nrows2 + ")");	
		}

		float M3[][] = new float[Nrows1][Ncols2];
		
		int	col, row;
		for(row=0; row<Nrows1; row++) {
			for(col=0; col<Ncols2; col++) {
				float	dotProd = 0f;
				int		i;
				for(i=0; i<Ncols1; i++) {
					dotProd += M1[row][i] * M2[i][col];
				}
				M3[row][col] = dotProd;
			}
				
		}
		return M3;
	}
	public static boolean isEqual(float[] v, float[] v2) {
		if (v==null)				return (v2==null);
		if (v.length!=v2.length)	return false;
		int	i;
		
		for(i=0; i<v.length; i++) {
			if (v[i] != v2[i]) {
				Logger.println("mismatch v[" + i + "]==" + v[i] + " v2[" + i + "]==" + v2[i]);
				return false;
			}
		}
		return true;
	}
	public static float[] transform(float[][] m, float[] v1) {
		
		if (numRows(m) == v1.length) {
			float	v2[] = new float[v1.length];
			int		row;
			for(row=0; row<v1.length; row++) {
				v2[row] = dotProduct(getRow(row, m), v1);
			}
			
			return v2;
		} else {
			throw new NumberFormatException("matrix cols does not match vector length (M * V)");
		}
		
	}
	public static float[] transform(float[] v1, float[][] m) {
		if (numRows(m) == v1.length) {
			float	v2[] = new float[v1.length];
			int		col;
			for(col=0; col<v1.length; col++) {
				v2[col] = dotProduct(getRow(col, m), v1);
			}
			return v2;
		} else {
			throw new NumberFormatException("vector length does not match matrix cols (V * M)");
		}
	}
		
	public static float	determinant(float[][] m) throws Exception {
		int	nRows = numRows(m);
		int nCols = numCols(m);
		if (nCols==nRows) {
			int		i, j;
			float	prodL = 1f;
			float	prodR = 1f;
			for(i=0; i<nRows; i++) {
				for(j=0; j<nRows; j++) {
					prodL *= m[i][(i+j)%nRows];
					prodR *= m[i][(i-j+nRows)%nRows];
				}
			}
			return prodL - prodR;
		} else {
			throw new Exception("Algebra.determinant(float m[][]) - m is not square");
		}
	}
};
